#ifndef linux
#define linux 1
#endif
#ifndef __x86_64__ 
#define __x86_64__ 1
#endif

#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wcast-qual"
#pragma GCC diagnostic ignored "-Wzero-as-null-pointer-constant"
#pragma GCC diagnostic ignored "-Wenum-compare"
#pragma GCC diagnostic ignored "-Wdouble-promotion"
#pragma GCC diagnostic ignored "-Wunknown-pragmas"
#pragma GCC diagnostic ignored "-Wdeprecated-declarations"
#pragma GCC diagnostic ignored "-Wreorder"
#include <buola/cv/ccloudview.h>
#include <buola/app/ceventloop.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/io/pcd_io.h>
#include <buola/mat/transformations.h>
#include <buola/cv/coccupancyoctree.h>
#include <buola/mat/cgaussianprocess.h>
#include <pcl/io/pcd_io.h>
#include <pcl/io/openni_grabber.h>
#include <buola/image/mat/mat_image.h>
#pragma GCC diagnostic pop


using namespace buola;

static pcl::PointCloud<pcl::PointXYZRGBA> gPCL;
static bool gDone=false;

void cloud_cb(const pcl::PointCloud<pcl::PointXYZRGBA>::ConstPtr &pCloud)
{
    static int lCount=0;
    
    if (++lCount == 30)
    {
        gPCL=*pCloud;
        gDone=true;
    }
}

pcl::PointCloud<pcl::PointXYZRGBA> grab_cloud()
{
    pcl::Grabber *lInterface=new pcl::OpenNIGrabber();
    lInterface->registerCallback(boost::function<void (const pcl::PointCloud<pcl::PointXYZRGBA>::ConstPtr&)>(cloud_cb));
    lInterface->start();

    while(!gDone)
    {
        sleep(chrono::milliseconds(20));
    }

    lInterface->stop();
    delete lInterface;
    return gPCL;
}

int main(int pNArg,char **pArgs)
{
    auto lPCL=grab_cloud();
    msg_info() << "got image!\n";
    
    mat::CVec3d l1{0.349425,0.13863,0.997};
    mat::CVec3d l2{-0.0957829,0.155234,0.867};
    mat::CVec3d l3{-0.0709429,0.2292,0.573};

    mat::CVec3d lTableX{1,0,0};
    mat::CVec3d lTableZ=-normalize(cross(l3-l1,l2-l1));
    mat::CVec3d lTableY=normalize(cross(lTableZ,lTableX));
    lTableX=cross(lTableY,lTableZ);

    mat::CVec3d lTableO=l3-0.15*lTableX+0.15*lTableY-0.2*lTableZ;

    mat::CMat34d lTrans=mat::transform_3d(lTableO,lTableX,lTableY,lTableZ);
    mat::CMat34d lTransI=inverse(lTrans);

    msg_info() << lTrans << lTransI << "\n";

    msg_info() << lTransI*mat::CVec3d(lTableO+lTableX) << "\n";
    
    msg_info() << lTableX << lTableY << lTableZ << "\n";
    
    buola_init(pNArg,pArgs);

    try
    {
        cvi::CCloud_rgb8 lCloud;
        start_timer();
        for(int i=0;i<lPCL.size();i++)
        {
            if(!is_nan(lPCL[i].x))
            {
                cvi::CCloud_rgb8::TPoint lPoint;
                lPoint.mPos=lTransI*mat::CVec3d(lPCL[i].x,lPCL[i].y,lPCL[i].z);
                if(lPoint.mPos.x()<0||lPoint.mPos.x()>0.6||lPoint.mPos.y()<0||lPoint.mPos.y()>0.5||lPoint.mPos.z()<-0.1) continue;
                lPoint.mData=img::pixel(lPCL[i].r,lPCL[i].g,lPCL[i].b);
                lCloud.push_back(lPoint);
            }
        }
        end_timer();

        lCloud.SetOrigin(lTransI*mat::CVec3d(0,0,0));

        cvi::COccupancyOcTree lTree(0.005);

        start_timer();
        lTree.InsertCloud(lCloud);
        end_timer();

        for(auto i=lTree.begin();i!=lTree.end();++i)
        {
            cvi::CCloud_rgb8::TPoint lP;

            lP.mPos=i.Coords();

            if(i->Value()>0)
            {
                lP.mData[0]=255;
                lP.mData[1]=0;
                lP.mData[2]=0;
            }
            else
            {
                lP.mData[0]=127;
                lP.mData[1]=127;
                lP.mData[2]=127;
            }

            lCloud.push_back(lP);
        }

        start_timer();
        mat::CMat_d lHeightMap=cvi::get_height_map_mat(lTree,{100,100},0,0.5,0,0.5,-0.1,0.5);
        end_timer();
        start_timer();
        save(img::mat_image(lHeightMap,-0.1,0.5),io::file("height_map.ppm"));
        end_timer();
        ///\todo investigate nInf
        mat::CVec3d lTallest(0,0,-INFINITY);
        
        ///\todo check if it is possible to do a count if not with some adaptor
        int r=count_if(lHeightMap,is_finite<double>);
        mat::CMat_d lPoints(r,2);
        mat::CVec_d lValues(r);

        msg_info() << "r=" << r << "\n";    
        
        r=0;
        for(int x=0;x<100;x++)
        {
            for(int y=0;y<100;y++)
            {
                if(!is_finite(lHeightMap(x,y))) continue;
                ///\todo lPoints.Row(r)={x*0.006,y*0.006};
                ///\todo lPoints.Row(r).x()=x*0.006;
                lPoints(r,0)=x*0.005;
                lPoints(r,1)=y*0.005;
                lValues[r]=lHeightMap(x,y);

                ++r;
                
                if(lHeightMap(x,y)>lTallest.z())
                    lTallest={x*0.005,y*0.005,lHeightMap(x,y)};
            }
        }

        mat::CMat34d lKinectToRobot=mat::rotation_translation_3d(
                        mat::CQuat_d(-0.18138060909146622,0.20938222550806626,0.76739448403667976,-0.57824377571831764),
                        mat::CVec3d(0.56611077880859375,1.227518310546875,0.3039049072265625));

        mat::CVec3d l20down=lKinectToRobot*lTrans*mat::CVec3d(lTallest+mat::CVec3d(0,0,-0.2));//+0.2*nZ
        msg_info() << "20 cms down:\n" << l20down << "\n";
        mat::CVec3d l20right=lKinectToRobot*lTrans*mat::CVec3d(lTallest+mat::CVec3d(0.2,0,0));
        msg_info() << "20 cms right:\n" << l20right << "\n";
        
        msg_info() << "tallest point in table space: \n" << lTallest << "\n";
        lTallest=lTrans*lTallest;
        msg_info() << "tallest point in kinect space: \n" << lTallest << "\n";
        
        lTallest=lKinectToRobot*lTallest;
        msg_info() << "tallest point in robot space: \n" << lTallest << "\n";
        
        msg_info() << "size of height map:" << r << "\n";

        msg_info() << "training gaussian process\n";
        start_timer();
        mat::CGaussianProcess<double> lProcess(lPoints,lValues,1073.0,0.0013,0.04);
        end_timer();

        msg_info() << "making prediction\n";
        start_timer();
        mat::CMat_d lPredictedMap(100,100);
        for(int x=0;x<100;x++)
        {
            for(int y=0;y<100;y++)
            {
                lPredictedMap(x,y)=lProcess.F(mat::CVec2d(x*0.005,y*0.005));
            }
        }
        end_timer();

        start_timer();
        mat::CMat_d lVarianceMap(100,100);
        for(int x=0;x<100;x++)
            for(int y=0;y<100;y++)
                lVarianceMap(x,y)=lProcess.V(mat::CVec2d(x*0.005,y*0.005));
        end_timer();

        msg_info() << "variance-map:\n" << lVarianceMap << "\n";
        
        save(img::mat_image(lPredictedMap,-0.1,0.5),io::file("predicted_map.ppm"));
        save(img::mat_image(lVarianceMap,0.0,0.04),io::file("variance_map.ppm"));

        msg_info() << "height-map:\n" << lHeightMap << "\n";
        
        msg_info() << "log-likelihood=" << lProcess.LogLikelihood() << "\n";

        gui::CWindow lWindow;
        lWindow.Create(nullptr,CSize_d(800,600));
        cvi::CCloudView lView;
        lView.Create(&lWindow);
        lView.SetAxes({0,0,0},{1,0,0},{0,1,0},{0,0,1});
        mat::CVec3d lCenter=center(lCloud);
        lView.SetCloud(std::move(lCloud));
        lView.SetCameraPos(lCenter-mat::CVec3d({30,0,0}));
        attach(&lView,gui::ATTACH_REPLACECLIENT);
        lView.Show();
        lWindow.Show();

        set_main_window(&lWindow);

        get_main_event_loop().Loop();
    }
    catch(std::exception &pE)
    {
        msg_info() << "caught exception " << pE.what() << "\n";
    }

    return buola_finish();
}
